#include <plib.h>
#include "system.h"
#include "gp2d12.h"

unsigned int rawData;
double distance;

void initializeGP2D12(){
    // Intitialize distance!
    distance = 0.0;

    // configure and enable the ADC
    CloseADC10();	// ensure the ADC is off before setting the configuration

    // define setup parameters for OpenADC10
    // 				Turn module on | ouput in integer | trigger mode auto | enable autosample
    DWORD PARAM1 = ADC_FORMAT_INTG | ADC_CLK_AUTO | ADC_AUTO_SAMPLING_ON;

    // define setup parameters for OpenADC10
    // 				ADC ref external    | disable offset test    | disable scan mode | perform 2 samples | use dual buffers | use alternate mode
    DWORD PARAM2 = ADC_VREF_AVDD_AVSS | ADC_OFFSET_CAL_DISABLE | ADC_SCAN_OFF | ADC_SAMPLES_PER_INT_1 | ADC_BUF_8 | ADC_ALT_BUF_OFF | ADC_ALT_INPUT_OFF;

    // define setup parameters for OpenADC10
    //				  use ADC internal clock | set sample time
    DWORD PARAM3 = ADC_CONV_CLK_INTERNAL_RC | ADC_SAMPLE_TIME_1 | (0x00FF << _AD1CON3_ADCS_POSITION);


    // define setup parameters for OpenADC10
    //               set AN4 and AN5 as analog inputs
    DWORD PARAM4 = 0;
    DWORD PARAM6 = 0;
    //ENABLE_AN4_ANA | ENABLE_AN5_ANA
    switch(GP2D12_CHANNEL){
        case 0: PARAM4 = ENABLE_AN0_ANA, PARAM6 = ADC_CH0_NEG_SAMPLEA_NVREF | ADC_CH0_POS_SAMPLEA_AN0; break;
        case 1: PARAM4 = ENABLE_AN1_ANA, PARAM6 = ADC_CH0_NEG_SAMPLEA_NVREF | ADC_CH0_POS_SAMPLEA_AN1; break;
        case 2: PARAM4 = ENABLE_AN2_ANA, PARAM6 = ADC_CH0_NEG_SAMPLEA_NVREF | ADC_CH0_POS_SAMPLEA_AN2; break;
        case 3: PARAM4 = ENABLE_AN3_ANA, PARAM6 = ADC_CH0_NEG_SAMPLEA_NVREF | ADC_CH0_POS_SAMPLEA_AN3; break;
        case 4: PARAM4 = ENABLE_AN4_ANA, PARAM6 = ADC_CH0_NEG_SAMPLEA_NVREF | ADC_CH0_POS_SAMPLEA_AN4; break;
        case 5: PARAM4 = ENABLE_AN5_ANA, PARAM6 = ADC_CH0_NEG_SAMPLEA_NVREF | ADC_CH0_POS_SAMPLEA_AN5; break;
        case 6: PARAM4 = ENABLE_AN6_ANA, PARAM6 = ADC_CH0_NEG_SAMPLEA_NVREF | ADC_CH0_POS_SAMPLEA_AN6; break;
        case 7: PARAM4 = ENABLE_AN7_ANA, PARAM6 = ADC_CH0_NEG_SAMPLEA_NVREF | ADC_CH0_POS_SAMPLEA_AN7; break;
        case 8: PARAM4 = ENABLE_AN8_ANA, PARAM6 = ADC_CH0_NEG_SAMPLEA_NVREF | ADC_CH0_POS_SAMPLEA_AN8; break;
        case 9: PARAM4 = ENABLE_AN9_ANA, PARAM6 = ADC_CH0_NEG_SAMPLEA_NVREF | ADC_CH0_POS_SAMPLEA_AN9; break;
        case 10: PARAM4 = ENABLE_AN10_ANA, PARAM6 = ADC_CH0_NEG_SAMPLEA_NVREF | ADC_CH0_POS_SAMPLEA_AN10; break;
        case 11: PARAM4 = ENABLE_AN11_ANA, PARAM6 = ADC_CH0_NEG_SAMPLEA_NVREF | ADC_CH0_POS_SAMPLEA_AN11; break;
        case 12: PARAM4 = ENABLE_AN12_ANA, PARAM6 = ADC_CH0_NEG_SAMPLEA_NVREF | ADC_CH0_POS_SAMPLEA_AN12; break;
        case 13: PARAM4 = ENABLE_AN13_ANA, PARAM6 = ADC_CH0_NEG_SAMPLEA_NVREF | ADC_CH0_POS_SAMPLEA_AN13; break;
        case 14: PARAM4 = ENABLE_AN14_ANA, PARAM6 = ADC_CH0_NEG_SAMPLEA_NVREF | ADC_CH0_POS_SAMPLEA_AN14; break;
        case 15: PARAM4 = ENABLE_AN15_ANA, PARAM6 = ADC_CH0_NEG_SAMPLEA_NVREF | ADC_CH0_POS_SAMPLEA_AN15; break;
        default: PARAM4 = ENABLE_ALL_DIG; break;
    }

    if(PARAM4 == ENABLE_ALL_DIG)
        return;

    // define setup parameters for OpenADC10
    // do not assign channels to scan
    DWORD PARAM5 = SKIP_SCAN_ALL;

    // use ground as neg ref for A | use AN4 for input A      |  use ground as neg ref for A | use AN5 for input B

    // configure to sample AN4 & AN5
    SetChanADC10(PARAM6); // configure to sample AN4 & AN5
    OpenADC10( PARAM1, PARAM2, PARAM3, PARAM4, PARAM5 ); // configure ADC using the parameters defined above

    ConfigIntADC10(ADC_INT_PRI_3 | ADC_INT_SUB_PRI_2 | ADC_INT_ON);

    EnableADC10(); // Enable the ADC

}

double getGP2D12Distance(){
    return distance;
}

void updateGP2D12(){
    INTEnable(INT_AD1, INT_ENABLED);
}

void __ISR(_ADC_VECTOR, ipl3) interruptGP2D12(void){

    mAD1ClearIntFlag();

    rawData = ReadADC10(8 * ((~ReadActiveBufferADC10() & 0x01))); // ADC1BUF0;
    
    double volts = (3.3/1024.0)*rawData;
    distance = (25.0/(volts-0.13)) - 0.42;
    if(distance <= 1e-5) distance = 1000.0;

    PORTAbits.RA5 = distance > 30.0;

    while(IFS1bits.AD1IF){
        int i = ADC1BUF0;
        mAD1ClearIntFlag();
    }

    INTEnable(INT_AD1, INT_DISABLED);

}